

#include <ctype.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <sstream>
#include <pthread.h>
#include <glib.h>

#include <ros/ros.h>

#include <gtk/gtk.h>
#include <gdk/gdkkeysyms.h>
#include <ros/package.h>

#include <GL/glu.h>
#include <GL/glut.h>
#include <unistd.h>


#include <image_transport/subscriber.h>
#include <image_transport/image_transport.h>
#include <geometry_msgs/Twist.h>

#include <cv_bridge/CvBridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <boost/thread.hpp>
#include <boost/format.hpp>
#include "boost/thread/mutex.hpp"

//#include "window_thread.h"

void imageCb               (const sensor_msgs::ImageConstPtr& msg);
void start_saving          (GtkWidget *widget, gpointer data);
void chooseDirectory       (GtkWidget *widget, gpointer data);
gboolean handleMouseMotion (GtkWidget *widget, GdkEventMotion *event, gpointer data);
gboolean handleMousePress  (GtkWidget *widget, GdkEventButton *event, gpointer data);
gboolean handleKeyPress    (GtkWidget *widget, GdkEventKey *event, gpointer data);
void exitProgram           (GtkWidget *widget, gpointer data);
void force_refresh         (const ros::TimerEvent&);

static const char WINDOW_NAME[] = "Kinect View";

image_transport::Subscriber sub_;
sensor_msgs::CvBridge img_bridge_;

boost::mutex image_mutex_;
sensor_msgs::ImageConstPtr last_msg_;
cv::Mat last_image_;

std::string window_name_= WINDOW_NAME;
boost::format filename_format_;
std::string default_saving_folder_ = "./images/image_";
std::string default_saving_extension_ = ".jpg";
int count_;

GtkWidget *window;
GtkWidget *vbox;
GtkWidget *toolbar;
GtkWidget *imageWidget;
GtkWidget *fileChooser;

bool RECORD_ = false;
int nb_ = 1;

float angular_ = 0.0f;
float linear_ = 0.0f;
float speedrate_ = 0.8f;

int padding_ = 20;

int callCounter = 0;

ros::Publisher vel_pub_;
boost::mutex publish_mutex_;

int main(int argc, char **argv) {
	ros::init(argc, argv, "manager", ros::init_options::AnonymousName);
	ROS_WARN("##Je demarre");

	//ros::NodeHandle nh =ros::this_node;
	ros::NodeHandle n;
	ros::NodeHandle nh("~");

	// Command line argument parsing
	// First positional argument is the transport type
	std::string transport = "raw";
	for (int i = 0; i < argc; ++i) {
		if (argv[i][0] != '-') {
			transport = argv[i];
			break;
		}
	}
	// Internal option, should be used only by the image_view node
	//bool shutdown_on_close = std::find(argv.begin(), argv.end(),
		                     //"--shutdown-on-close") != argv.end();

	/*
	std::string format_string;
	local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
	filename_format_.parse(format_string);

	//cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this);
	//*/
	//cv::namedWindow(window_name_);

	// INIT WINDOW
	g_thread_init(NULL);
	gdk_threads_init();
	gtk_init(&argc, &argv);

	window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
	gtk_window_set_position(GTK_WINDOW(window), GTK_WIN_POS_CENTER);
	gtk_window_set_default_size(GTK_WINDOW(window), 250, 200);
	gtk_window_set_title(GTK_WINDOW(window), window_name_.c_str());

	vbox = gtk_vbox_new(FALSE, 0);
	gtk_container_add(GTK_CONTAINER(window), vbox);

	// FILE CHOOSER
	fileChooser = gtk_file_chooser_dialog_new("SÃ©lection d'un rÃ©pertoire d'enregistrement", NULL,
                                         GTK_FILE_CHOOSER_ACTION_SELECT_FOLDER,
                                         GTK_STOCK_CANCEL, GTK_RESPONSE_CANCEL,
                                         GTK_STOCK_OPEN, GTK_RESPONSE_ACCEPT,
                                         NULL);
	gtk_file_chooser_set_filename(GTK_FILE_CHOOSER(fileChooser), default_saving_folder_.c_str()); // Default folder

	// MENUBAR
	GtkWidget *menubar;
	GtkWidget *filemenu;
	GtkWidget *file;
	GtkWidget *quit;

	menubar = gtk_menu_bar_new();
	filemenu = gtk_menu_new();

	file = gtk_menu_item_new_with_label("Fichier");
	quit = gtk_menu_item_new_with_label("Quitter");

	gtk_menu_item_set_submenu(GTK_MENU_ITEM(file), filemenu);
	gtk_menu_shell_append(GTK_MENU_SHELL(filemenu), quit);
	gtk_menu_shell_append(GTK_MENU_SHELL(menubar), file);
	gtk_box_pack_start(GTK_BOX(vbox), menubar, FALSE, FALSE, 3);

	// g_signal_connect_swapped(G_OBJECT(window), "destroy",
	//     G_CALLBACK(gtk_main_quit), NULL);

	g_signal_connect(G_OBJECT(quit), "activate",
	G_CALLBACK(gtk_main_quit), NULL);

	// TOOLBAR
	GtkToolItem *newa;
	GtkToolItem *open;
	GtkToolItem *save;
	GtkToolItem *sep;
	GtkToolItem *exit;

	toolbar = gtk_toolbar_new();
	gtk_toolbar_set_style(GTK_TOOLBAR(toolbar), GTK_TOOLBAR_ICONS);

	gtk_container_set_border_width(GTK_CONTAINER(toolbar), 2);

	newa = gtk_tool_button_new_from_stock(GTK_STOCK_NEW);
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), newa, -1);
	g_signal_connect(G_OBJECT(newa), "clicked",
		G_CALLBACK(chooseDirectory), NULL);

	open = gtk_tool_button_new_from_stock(GTK_STOCK_OPEN);
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), open, -1);

	save = gtk_tool_button_new_from_stock(GTK_STOCK_SAVE);
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), save, -1);
	g_signal_connect(G_OBJECT(save), "clicked",
		G_CALLBACK(start_saving), NULL);

	sep = gtk_separator_tool_item_new();
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), sep, -1);

	exit = gtk_tool_button_new_from_stock(GTK_STOCK_QUIT);
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), exit, -1);

	gtk_box_pack_start(GTK_BOX(vbox), toolbar, FALSE, FALSE, 5);

	g_signal_connect(G_OBJECT(exit), "clicked",
	 	G_CALLBACK(gtk_main_quit), NULL);

	// ADD MOUSE MOVE EVENT HANDLING
	gtk_widget_set_events(window, GDK_POINTER_MOTION_HINT_MASK | GDK_POINTER_MOTION_MASK
									 | GDK_BUTTON_PRESS_MASK | GDK_BUTTON_RELEASE_MASK | GDK_KEY_PRESS_MASK | GDK_KEY_RELEASE_MASK);
	g_signal_connect(G_OBJECT(window), "motion-notify-event",
	 	G_CALLBACK(handleMouseMotion), NULL);

	g_signal_connect(G_OBJECT(window), "button-press-event",
	 	G_CALLBACK(handleMousePress), NULL);

	g_signal_connect(G_OBJECT(window), "button-release-event",
	 	G_CALLBACK(handleMousePress), NULL);

	// ADD KEY PRESS EVENT HANDLING
	g_signal_connect(G_OBJECT(window), "key-press-event",
	 	G_CALLBACK(handleKeyPress), NULL);

	g_signal_connect(G_OBJECT(window), "key-release-event",
	 	G_CALLBACK(handleKeyPress), NULL);

	//gtk_widget_set_events(window, GDK_POINTER_MOTION_HINT_MASK);

	g_signal_connect_swapped(G_OBJECT(window), "destroy",
		G_CALLBACK(exitProgram), NULL);

	gtk_widget_show_all(window);

	// CREATE ROS TIMER
	ros::Timer timer = n.createTimer(ros::Duration(0.1), force_refresh);

	// Start the OpenCV window thread so we don't have to waitKey() somewhere
	//startWindowThread();

	// Subscribe to ros topic
	image_transport::ImageTransport it(nh);
	//sub_ = it.subscribe(topic, 1, &imageCb, this, transport);
	sub_ = it.subscribe("/image", 3, &imageCb);

	// Create publisher
	vel_pub_ =  n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

	ROS_WARN("####spin");
	ros::spin();

	gdk_threads_enter();
	gtk_main();
	gdk_threads_leave();

	return 0;
}

void force_refresh(const ros::TimerEvent&) {
	boost::mutex::scoped_lock lock(publish_mutex_);
	geometry_msgs::Twist vel;
	vel.angular.z = angular_;
	vel.linear.x = linear_;
	vel_pub_.publish(vel);
}

gboolean handleKeyPress(GtkWidget *widget, GdkEventKey *event, gpointer data) {
	if(imageWidget != NULL) {

		if(event->type == GDK_KEY_PRESS) {
			// LEFT
			if(event->keyval == 65361) {
				angular_ = 1.50;
			}
			// RIGHT
			else if(event->keyval == 65363) {
				angular_ = -1.50;
			}
			// TOP
			else if(event->keyval == 65362) {
				linear_ = 0.5 * speedrate_;
			}
			// BOTTOM
			else if(event->keyval == 65364) {
				ROS_WARN("BAS");
				linear_ = -0.5 * speedrate_;
			}
		}
		else if(event->type == GDK_KEY_RELEASE) {
			linear_ = 0.0;
			angular_ = 0.0;
		}

		boost::mutex::scoped_lock lock(publish_mutex_);
		geometry_msgs::Twist vel;
		vel.angular.z = angular_;
  		vel.linear.x = linear_;
		vel_pub_.publish(vel);
	}

	return TRUE;
}

gboolean handleMouseMotion(GtkWidget *widget, GdkEventMotion *event, gpointer data) {

	if(imageWidget != NULL) {
		gdouble xImgWidgetpos   = imageWidget->allocation.x;
		gdouble yImgWidgetpos   = imageWidget->allocation.y;
		gdouble imgWidgetWidth  = imageWidget->allocation.width;
		gdouble imgWidgetHeight = imageWidget->allocation.height;

		if(event->x >= xImgWidgetpos && event->x <= (imgWidgetWidth + xImgWidgetpos)) {
			if(event->y >= yImgWidgetpos && event->y <= (imgWidgetHeight + yImgWidgetpos)) {
				//calcul de la vitesse de rotation par rapport Ã  la position de la souris
				//
				int left_max_   = xImgWidgetpos + padding_;
				int right_max_  = (imgWidgetWidth + xImgWidgetpos) - padding_;
				int left_zero_  = (imgWidgetWidth + xImgWidgetpos) / 2 - padding_;
				int right_zero_ = (imgWidgetWidth + xImgWidgetpos) / 2 + padding_;

				//souris au centre
				if(event->x >= left_zero_ && event->x<= right_zero_)
				{
					angular_ = 0.0f;
				}

				//souris au mayimum Ã  droit ou Ã  gauche
				else if(event->x < left_max_ || event->x > right_max_)
				{
					if(event->x < left_max_)
					{
						//gauche
						angular_ = 1.5f;
					}
					else
					{
						//droite
						angular_ = -1.5f;
					}
				}

				//souris cÃ´tÃ© gauche entre le centre et le may
				else if(event->x>=left_max_ && event->x<left_zero_)
				{
					angular_ = (1.0f - (((event->x - left_max_)*1.0f) / ((left_zero_ - left_max_)*1.0f))) *1.5f;
				}

				//souris cÃ´tÃ© droit entre le centre et le may
				else if(event->x<right_max_ && event->x>=right_zero_)
				{
					angular_ = (((event->x - right_zero_)*1.0f) / ((right_max_ - right_zero_)*1.0f)) *-1.5f;
				}

				boost::mutex::scoped_lock lock(publish_mutex_);
				geometry_msgs::Twist vel;
				vel.angular.z = angular_;
  				vel.linear.x = linear_;
				vel_pub_.publish(vel);
			}
			else {
				angular_ = 0;
				linear_ = 0;
			}
		}
		else {
			angular_ = 0;
			linear_ = 0;
		}
	}
}

gboolean handleMousePress  (GtkWidget *widget, GdkEventButton *event, gpointer data) {
if(imageWidget != NULL) {
		gdouble xImgWidgetpos  = imageWidget->allocation.x;
		gdouble yImgWidgetpos   = imageWidget->allocation.y;
		gdouble imgWidgetWidth  = imageWidget->allocation.width;
		gdouble imgWidgetHeight = imageWidget->allocation.height;

		if(event->x >= xImgWidgetpos && event->x <= (imgWidgetWidth + xImgWidgetpos)) {
			if(event->y >= yImgWidgetpos && event->y <= (imgWidgetHeight + yImgWidgetpos)) {
				if(event->type == GDK_BUTTON_RELEASE) {
					linear_ = 0.0;
				}
				else if(event->type == GDK_BUTTON_PRESS) {
					if(event->button == 1) {
						linear_ = 0.5*speedrate_;
					}
					else if(event->button == 3) {
						linear_ = -0.5*speedrate_;
					}
				}

				boost::mutex::scoped_lock lock(publish_mutex_);
				geometry_msgs::Twist vel;
				vel.angular.z = angular_;
  				vel.linear.x = linear_;
				vel_pub_.publish(vel);
			}
			else {
				angular_ = 0;
				linear_ = 0;
			}
		}
		else {
			angular_ = 0;
			linear_ = 0;
		}
	}
	return TRUE;
}

void start_saving(GtkWidget *widget, gpointer data) {
	RECORD_ = !RECORD_;
}

void chooseDirectory(GtkWidget *widget, gpointer data) {
	gtk_dialog_run(GTK_DIALOG(fileChooser));
}

//////////////////////////////////////////////////////////////////////////


void imageCb(const sensor_msgs::ImageConstPtr& msg) {
	//ROS_WARN("####Encoding:");
	ROS_WARN(msg->encoding.c_str());
	image_mutex_.lock();

	// May want to view raw bayer data, which CvBridge doesn't know about
	if (msg->encoding.find("bayer") != std::string::npos) {
    		last_image_ = cv::Mat(msg->height, msg->width, CV_8UC1,
									const_cast<uint8_t*>(&msg->data[0]), msg->step);
	}
	// We want to scale floating point images so that they display nicely
	else if(msg->encoding.find("F") != std::string::npos) {
		ROS_WARN("####Floating:");

		cv::Mat float_image_bridge = img_bridge_.imgMsgToCv(msg, "32FC1");
		cv::Mat_<float> float_image = float_image_bridge;
		float max_val = 0;

		for(int i = 0; i < float_image.rows; ++i) {
			for(int j = 0; j < float_image.cols; ++j) {
				max_val = std::max(max_val, float_image(i, j));
			}
		}

		if(max_val > 0) {
			float_image /= max_val;
		}
		last_image_ = float_image;
	}
	else {
		// Convert to OpenCV native BGR color
		try {
			last_image_ = img_bridge_.imgMsgToCv(msg, "bgr8");

		}
		catch (sensor_msgs::CvBridgeException& e) {
			ROS_ERROR("Unable to convert '%s' image to bgr8", msg->encoding.c_str());
		}
	}


  	// conversion
	if (!last_image_.empty()){
		IplImage* image = new IplImage(last_image_);
		//IplImage *gtkMask = NULL;

		// RECORDING KINECT IMAGES
		if(RECORD_ == true)
		{
			std::ostringstream oss;
			std::string s1 = default_saving_folder_.c_str();
			std::string s2 = default_saving_extension_.c_str();
			oss << s1 << nb_ << s2;

			nb_++;
			cvSaveImage(oss.str().c_str() ,image);
		}

		// REFRESHING KINECT IMAGE DISPLAY
		gdk_threads_enter();
		GdkPixbuf *pixbuf;

		//Create another image with the same stuff
		IplImage *gtkMask = cvCreateImage(cvSize(image->width, image->height),
										image->depth, image->nChannels);
		cvCvtColor(image, gtkMask, CV_BGR2RGB);//Copy the IplImage to RGB format
		image_mutex_.unlock();
		//Copy the IplImage data to a GdkPixbuf:
		pixbuf = gdk_pixbuf_new_from_data (
		        (guchar*)gtkMask->imageData,//Raw image data
		        GDK_COLORSPACE_RGB,                     //Colorspace
		        FALSE,                                  //Has alpha channel or not
		        gtkMask->depth,                         //Bits per pixel
		        gtkMask->width,                         //Width of image
		        gtkMask->height,                        //Height of image
		        gtkMask->widthStep,                     //Row stride
		        NULL,                                   //Not sure what this is
		        NULL                                    //Not sure what this is
		);

		if(callCounter == 0) {
		  	imageWidget = gtk_image_new_from_pixbuf(pixbuf);
		  	gtk_box_pack_start(GTK_BOX(vbox), imageWidget, TRUE, TRUE, 5);
	  		callCounter++;
		}
		else {
			gtk_image_set_from_pixbuf(GTK_IMAGE(imageWidget), pixbuf);
		}

		gtk_widget_show_all(window);

		gdk_flush();
		while(gtk_events_pending())
			  gtk_main_iteration();
		gdk_flush();

		gdk_threads_leave();

		if(gtkMask != NULL) {
			delete(image);
			cvReleaseImage(&gtkMask);
			delete(gtkMask);
		}
		if(pixbuf != NULL) {
			g_object_unref(pixbuf);
		}
		last_image_.release();

	}

}

void exitProgram (GtkWidget *widget, gpointer data) {
		boost::mutex::scoped_lock lock(publish_mutex_);
		geometry_msgs::Twist vel;
		vel.angular.z = 0.0;
		vel.linear.x = 0.0;
		vel_pub_.publish(vel);
}





/**
 * @struct arg
 * @brief Allow to pass the arguments of the program to a pthread.<br>
 * Allow to use ROS params even ROS is in a child thread of Gtk.
 */
struct arg
{
  int argc;
  char **argv;
};

void *startROS (void *);
//void gpsOdomCallback (const nav_msgs::Odometry::ConstPtr &);

 
//GtkWidget * window;

class AppData
{
public:

  GtkWidget * window;
  GtkWidget *map_box;
  GtkWidget *map_container;
  
  bool ros_param_read;
  bool widget_created;

  bool lock_view;
  bool draw_path;
  int map_zoom_max;
  int map_current_zoom;
  const char *repo_uri;
  const char *friendly_name;
  char *cachedir;
};



void imageCb               (const sensor_msgs::ImageConstPtr& msg);
void start_saving          (GtkWidget *widget, gpointer data);
void chooseDirectory       (GtkWidget *widget, gpointer data);
gboolean handleMouseMotion (GtkWidget *widget, GdkEventMotion *event, gpointer data);
gboolean handleMousePress  (GtkWidget *widget, GdkEventButton *event, gpointer data);
gboolean handleKeyPress    (GtkWidget *widget, GdkEventKey *event, gpointer data);
void exitProgram           (GtkWidget *widget, gpointer data);
void force_refresh         (const ros::TimerEvent&);

static const char WINDOW_NAME[] = "Kinect View";

image_transport::Subscriber sub_;
sensor_msgs::CvBridge img_bridge_;

boost::mutex image_mutex_;
sensor_msgs::ImageConstPtr last_msg_;
cv::Mat last_image_;

std::string window_name_= WINDOW_NAME;
boost::format filename_format_;
std::string default_saving_folder_ = "./images/image_";
std::string default_saving_extension_ = ".jpg";
int count_;

GtkWidget *window;
GtkWidget *vbox;
GtkWidget *toolbar;
GtkWidget *imageWidget;
GtkWidget *fileChooser;

bool RECORD_ = false;
int nb_ = 1;

float angular_ = 0.0f;
float linear_ = 0.0f;
float speedrate_ = 0.8f;

int padding_ = 20;

int callCounter = 0;

ros::Publisher vel_pub_;
boost::mutex publish_mutex_;
 


AppData *data;

/*
void gpsFixCallback (const gps_common::GPSFix::ConstPtr & msg)
{
  // **** get GTK thread lock
  gdk_threads_enter ();
  
  gint pixel_x,pixel_y;
  
  OsmGpsMapPoint * point = osm_gps_map_point_new_degrees(msg->latitude,msg->longitude);
  osm_gps_map_convert_geographic_to_screen(data->map, point, &pixel_x, &pixel_y);
     
  if (OSM_IS_GPS_MAP (data->map)){

		// **** Center map on gps data received
		if(data->lock_view)
		{
			update_uav_pose_osd(data->osd,TRUE, pixel_x, pixel_y);
			osm_gps_map_set_center (data->map, msg->latitude, msg->longitude);
		}
		else
		{
			update_uav_pose_osd(data->osd,FALSE, pixel_x, pixel_y);
			osm_gps_map_gps_clear(data->map);
		}
	
		// **** Add point to the track
		osm_gps_map_track_add_point (data->current_track, point);
  }
	
  // **** release GTK thread lock 
  gdk_threads_leave ();
}
//*/
/**
 * @fn void *startROS (void *user)
 * @brief ROS thread.
 * 
 * The main program wait until "ros_param_read" in order to allow the <br>
 * ROS params to be also the Gtk Window and Widgets params.
 * Then the ROS thread wait to the widgets creation before subcribing<br>
 * to any topics, avoid to call public widget function for a widget not<br>
 * yet created.
 */
void* startROS (void *user)
{
  if (user != NULL)
  {
    struct arg *p_arg = (arg *) user;

    ros::init (p_arg->argc, p_arg->argv, "gpsd_viewer");
    ros::NodeHandle n;

    std::string local_path;
    std::string package_path = ros::package::getPath ("test_gtk");
    ros::NodeHandle n_param ("~");
    XmlRpc::XmlRpcValue xml_marker_center;

    ROS_INFO ("Starting GPSD Viewer");

	 // -----------------------------------------------------------------      
    // **** allow widget creation
    data->ros_param_read = true;

    // **** wait to widget creation
//*
    while (!data->widget_created)
    {
      ROS_DEBUG ("Waiting widgets creation");
    }
    //*/

    // -----------------------------------------------------------------      
    // **** topics subscribing
	 ros::Subscriber fix_sub;
		// Subscribe to ros topic
		image_transport::ImageTransport it(nh);
		//sub_ = it.subscribe(topic, 1, &imageCb, this, transport);
		sub_ = it.subscribe("/image", 1, &imageCb);


    ROS_INFO ("Spinning");
    ros::spin ();
  }

  // **** stop the gtk main loop
  if (GTK_IS_WINDOW (data->window))
  {
    gtk_main_quit ();
  }
  pthread_exit (NULL);
}

/**
 * @fn int main (int argc, char **argv)
 * @brief Main program & Gtk thread.
 * 
 * Create window and all widgets, then set there parameters to be the <br>
 * ROS params.
 */
int main (int argc, char **argv)
{

	ROS_WARN("coucou");
  GtkBuilder *builder;
  GError *error = NULL;
  char gui_filename[FILENAME_MAX];
  int start_zoom = 15;


  struct arg param;
  param.argc = argc;
  param.argv = argv;

  pthread_t rosThread;

  // **** init threads 
  g_thread_init (NULL);
  gdk_threads_init ();
  gdk_threads_enter ();

  // **** init gtk 
  gtk_init (&argc, &argv);

  // **** get the glade gui file
  std::string package_path = ros::package::getPath ("test_gtk");
  sprintf (gui_filename, "%s/%s", package_path.c_str (), "gui.glade");

  // Allocate data structure
  data = g_slice_new (AppData);

  // Create new GtkBuilder object
  builder = gtk_builder_new ();
  // Load UI from file
  if (!gtk_builder_add_from_file (builder, gui_filename, &error))
  {
    g_warning ("%s", error->message);
    g_free (error);
    pthread_exit (NULL);
  }

  // Get main window pointer from UI
  data->window = GTK_WIDGET (gtk_builder_get_object (builder, "window"));

  // **** create ROS thread
  pthread_create (&rosThread, NULL, startROS, &param);

  // **** wait ros finish read params
  //*
  while (!data->ros_param_read)
  {
    ROS_DEBUG ("Waiting ROS params");
  }
//*/
  // Some initialisation
  gdk_window_set_debug_updates (false);


  // Add the map to the box
  //data->map_box = GTK_WIDGET (gtk_builder_get_object (builder, "hbox2"));
  //gtk_box_pack_start (GTK_BOX (data->map_box), GTK_WIDGET (data->map), TRUE, TRUE, 0);

  //data->map_container = GTK_WIDGET (gtk_builder_get_object (builder, "hbox1"));

  // Connect signals
  //gtk_builder_connect_signals (builder, data);

  // Destroy builder, since we don't need it anymore
  g_object_unref (G_OBJECT (builder));

  // Show window. All other widgets are automatically shown by GtkBuilder
  gtk_widget_show_all (data->window);

  // **** allow ROS spinning
  data->widget_created = true;
  
  // Start main loop
  gtk_main ();
  gdk_threads_leave ();
  return 0;
}
